#include "DarkGDK.h"
#include "Utilities.h"

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Calculate the distance between two points.
float getDistance ( float x1 , float y1 , float z1 , float x2 , float y2 , float z2)
{
	float dist;
	float dx;
	float dy;
	float dz;

	dx = x2 - x1;
	dy = y2 - y1;
	dz = z2 - z1;

	dist = sqrt ( ( dx * dx ) + ( dy * dy ) + ( dz * dz ) );

	return dist;
}

float getDistance ( const Triple & p1, const Triple & p2 )
{
	return getDistance( p1.x, p1.y, p1.z, p2.x, p2.y, p2.z );
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Return a point between p1 and p2, where t represents the weighting from 0 to 1
Triple interpolate( const Triple & p1, const Triple & p2, float t )
{
	float dx = p2.x - p1.x;
	float dy = p2.y - p1.y;
	float dz = p2.z - p1.z;
	Triple pt3;
	pt3.x = t * dx + p1.x;
	pt3.y = t * dy + p1.y;
	pt3.z = t * dz + p1.z;
	return pt3;
}

Pose interpolate( Pose p1, Pose p2, float t )
{
	Pose newPose;
	newPose.position = interpolate( p1.position, p2.position, t );

	// transform rotations so the shorter path is used.
	Triple dr;
	bool transX, transY, transZ;
	transX = transY = transZ = false;

	dr.x = p2.rotation.x - p1.rotation.x;
	if( fabs( dr.x ) > 180 )
	{
		transX = true;
		p2.rotation.x += ( dr.x > 0 ) ? -360 : 360;
	}

	dr.y = p2.rotation.y - p1.rotation.y;
	if( fabs( dr.y ) > 180 )
	{
		transY = true;
		p2.rotation.y += ( dr.y > 0 ) ? -360 : 360;
	}

	dr.z = p2.rotation.z - p1.rotation.z;
	if( fabs( dr.z ) > 180 )
	{
		transZ = true;
		p2.rotation.z += ( dr.z > 0 ) ? -360 : 360;
	}

	newPose.rotation = interpolate( p1.rotation, p2.rotation, t );

	// Make sure values are in the correct range
	if( transX )
		newPose.rotation.x = dbWrapValue( newPose.rotation.x );
	if( transY )
		newPose.rotation.y = dbWrapValue( newPose.rotation.y );
	if( transZ )
		newPose.rotation.z = dbWrapValue( newPose.rotation.z );

	return newPose;
}

//////////////////////////////////////////////////////////////////
// String functions from Amadeus on dreamincode.net

std::string StringToUpper(std::string strToConvert)
{//change each element of the string to upper case
   for(unsigned int i=0;i<strToConvert.length();i++)
   {
      strToConvert[i] = toupper(strToConvert[i]);
   }
   return strToConvert;//return the converted string
}

std::string StringToLower(std::string strToConvert)
{//change each element of the string to lower case
   for(unsigned int i=0;i<strToConvert.length();i++)
   {
      strToConvert[i] = tolower(strToConvert[i]);
   }
   return strToConvert;//return the converted string
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Turn one object to face another
int turnToFace( int source, int target )
{
	// source: object that we want to turn too 
	// target: camera or object to point too ( 0 = camera )

	// Returned values:
	//   0  : no turn needed
	//   -1  : turn left
	//   1 : turn right

	float	fOrignalAngleY = dbObjectAngleY ( source ); // store the original yaw of the target
	int		s;
	int		t;
	int		sum;

	// turn to face the target pivot
	if ( target == 0 ) // if 0 then it is the camera we are pointing too
	{
		dbPointObject ( source , dbCameraPositionX ( 0 ) , 0 , dbCameraPositionZ ( 0 ) );
	}
	else
	{
		dbPointObject ( source , dbObjectPositionX ( target ) , 0 , dbObjectPositionZ ( target ) );		
	}
	
	// start angle and end angle
	s = (int) dbWrapValue( fOrignalAngleY );
	t = (int) dbWrapValue( dbObjectAngleY ( source ) );

	// Put original Y rotation back
	dbYRotateObject ( source , fOrignalAngleY );
	
	( s > t ) ? sum = s - t : sum = t - s;

		
	if ( sum <= 4 ) return 0; // If the two angles are close we do not need any rotation !

	// the angle goes from 0,180 and 0,-180; now I normalize to 0-360
	//if ( s < 0 ) s = 360 + s;
	//if ( t < 0 ) t = 360 + t;

	// now we found the right direction where to turn, in order to choose the shortest path:

	// check if the difference is greather than 180
	if ( abs ( s - t ) > 180 )
	{
		// check if the start angle is greater than the target angle
		if ( s > t )
		{
			return 1; // turn left
		}
		else
		{
			return -1; //turn right
		}
	}
	else
	{
		// check if the start angle is greater than the target angle
		if ( s > t )
		{
			return -1; //turn right
		}
		else
		{
			return  1; //turn left
		}
		
	}

}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Show the number of frames per second
void 
showFPS ( )
{
	char	czText[256];
	char	czFPS[10];

	wsprintf ( czFPS , "%d FPS " , dbScreenFPS ( ) );
	dbText ( 0 , 0 , czFPS );
}
